#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include "fanguangbantrilateration/matchcostmapwithscan_srvs.h"
#include "fanguangbantrilateration/matchcostmapwithscan_result.h"
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/transform_broadcaster.h>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <thread>
#include <map>
#include <mutex>
/**
 * 雷达匹与周边特征的配情况
 * 此版本是针对长走廊环境设计的一款反光柱定位产品，在窄长走廊中，由于空间限制只能一维部署反光柱，难以具备唯一特征。
 * 它需要在无人的情况下，而且雷达能够扫描到两边时定位效果比较好。
 * 如果不能保证雷达扫描到墙体就需要重新设计的方式的定位策略。
 *
 * 苏凯
 * 2025-2-8
 * 获取到当前移动机器人是否在自由去，雷达与地图轮廓耦合比例是否正常
 * 地图 填充网格数据，0代表空闲，100代表占用，-1代表未知
 * rosrun fanguangbantrilateration trilateration_matchCostmapWithScan_node
   int   free_cell_ = 0;// 0代表空闲
   int   occupied_cell_ = 100;//100代表占用
   int   unknown_cell_ = -1;// -1代表未知
 */

// 定义颜色的转义序列 std::cerr
const std::string RED = "\033[31m";
const std::string GREEN = "\033[32m";
const std::string YELLOW = "\033[33m";
const std::string BLUE = "\033[34m";
const std::string MAGENTA = "\033[35m";
const std::string CYAN = "\033[36m";
const std::string RESET = "\033[0m";

//laser_geometry::LaserProjection projector_;
tf::TransformListener* tf_listener_;
ros::Publisher initial_pose_pub;
nav_msgs::OccupancyGridConstPtr map_;  // 存储地图数据
std::mutex scancallback_mutex_;

std::string map_frame_id_="map";//地图坐标系
std::string base_link_frame_id_="base_link";//小车坐标系
std::string odom_frame_id_="odom";//odom坐标系
std::string laser_frame_id_="laser";// base_scan laser  雷达坐标系
std::string imu_frame_id_="imu_link";//imu坐标系

//toptic
std::string scan_toptic_name_="/scan";
std::string map_toptic_name_="/map";
std::string scan_points_map_toptic_name_="/scan_points_map";
std::string map_points_toptic_name_="/map_points";
std::string initialpose_toptic_name_="/initialpose";
std::string service_name_="/match_costmap_with_scan_service";
std::string getResultMapvalue_result_toptic_name_="/match_costmap_resultMapvalue";
std::string getCalculateScanMapRatio_result_toptic_name_="/match_costmap_calculateScanMapRatio";

bool iswileTure_=true;
int   free_cell_ = 0;// 0代表空闲
int   occupied_cell_ = 100;//100代表占用
int   unknown_cell_ = -1;// -1代表未知

//todo 2. 雷达匹配情况
double radius_ = 0.1;//搜索半径
double threshold_ = 0.8;//比例阀值，超过这个比例认为雷达匹配的挺好

double result_Mapvalue_=-3;//代价值
int total_points_ = 0;//雷达点数据量的有效数据量
int matching_points_ = 0;
int scan_points_map_size_=0;
double ratio_=0.0;
bool is_ratio_ =false;
bool is_use_scan_points_map_size_ =true;//true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。false 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。
bool isPublish_matchcostmapwithscan_mssage_=true;//是否发布数据
// 获取laser到map的变换
tf::StampedTransform base_link_to_laser_transform_;//base_linkTlaser

//创建一个宏，用于自动生成键值对：
#define ADD_HANDLER(handlers, handler) \
    handlers[#handler] = handler;
// service server 使用函数指针类型别名定义带有参数的函数指针类型别名，using：别名
using HandlerFunction = void (*)(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request&, fanguangbantrilateration::matchcostmapwithscan_srvs::Response&);
// 创建request_handlers
std::map<std::string, HandlerFunction>  request_handlers_;
//创建一个函数，用于填充 request_handlers 映射表
std::map<std::string, HandlerFunction> createRequestHandlers();
//查找请求类型对应的处理函数,默认使用函数名即为 request_handlers_ 中的 key
void processRequest(const std::string& request_type, const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& res);


struct Point2D {
    double x=0.0, y=0.0,z=0.0;
};

// 行优先 nav_msgs::OccupancyGrid从地图数据生成轮廓点云,
std::vector<Point2D> map_points_ ;
//nav_msgs::OccupancyGrid& map的行优先数据
std::vector<int> generateMapvalues_;

ros::Publisher scan_points_pub_,scan_points_trilateration_pub_,getResultMapvalue_result_pub_,getCalculateScanMapRatio_result_pub_;





//=======================================

// 定义一个函数用于转换
tf::Transform poseToTransform(const geometry_msgs::Pose& pose) {
    // 创建 tf::Transform 实例
    tf::Transform transform;

    // 设置位置
    transform.setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));

    // 设置旋转
    tf::Quaternion q(
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w
    );
    transform.setRotation(q);

    return transform;
}
// 定义一个函数用于转换
geometry_msgs::Pose transformToPose(const tf::Transform& transform) {
    // 创建 geometry_msgs::Pose 实例
    geometry_msgs::Pose pose;

    // 设置位置
    pose.position.x = transform.getOrigin().x();
    pose.position.y = transform.getOrigin().y();
    pose.position.z = transform.getOrigin().z();

    // 设置旋转
    pose.orientation.x = transform.getRotation().getX();
    pose.orientation.y = transform.getRotation().getY();
    pose.orientation.z = transform.getRotation().getZ();
    pose.orientation.w = transform.getRotation().getW();

    return pose;
}

//===================================




// 函数：将Point2D格式的数据转换为sensor_msgs::PointCloud2
sensor_msgs::PointCloud2 convertToPointCloud2(const std::vector<Point2D>& points) {
    sensor_msgs::PointCloud2 cloud_msg;
    cloud_msg.header.stamp = ros::Time::now();
    cloud_msg.header.frame_id = map_frame_id_;

    cloud_msg.height = 1;  // 点云为无序点
    cloud_msg.width = points.size();
    cloud_msg.is_dense = false;
    // 设置字段（x, y）
    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
//    modifier.setPointCloud2FieldsByString(1, "xy");



    // 设置字段（x, y）
//    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
//    modifier.setPointCloud2Fields(2,"x", 1, sensor_msgs::PointField::FLOAT32,"y", 1, sensor_msgs::PointField::FLOAT32);
//    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
//    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");

    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(points.size());
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");


    for (const auto& point : points) {

        *iter_x = point.x;
        *iter_y = point.y;
        *iter_z =0.0;
        ++iter_x;
        ++iter_y;
        ++iter_z;
    }

    // 打印点云数据
    // 注意这里是演示打印数据，实际使用中应该避免在每个循环中打印
    iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud_msg, "x");
    iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud_msg, "y");
   // ROS_INFO("PointCloud2 Data:");
    for (size_t i = 0; i < points.size(); ++i) {
        //   std::cout << "x:" << points[i].x << " y:" << points[i].y << std::endl;
        //   ROS_INFO("Point %zu: iter_x = %f, iter_y = %f", i, *iter_x, *iter_y);
        ++iter_x;
        ++iter_y;
    }


    return cloud_msg;
}
///================================================

// 将数据转换为Point2D格式
std::vector<Point2D> convertToPoint2D(const sensor_msgs::LaserScan::ConstPtr& scan) {
    std::vector<Point2D> points;
    int num_points = (scan->angle_max - scan->angle_min) / scan->angle_increment;

    for (int i = 0; i < num_points; ++i) {
        if (scan->ranges[i] < scan->range_max && scan->ranges[i] > scan->range_min) {
            Point2D point;
            point.x = scan->ranges[i] * cos(scan->angle_min + i * scan->angle_increment);
            point.y = scan->ranges[i] * sin(scan->angle_min + i * scan->angle_increment);
            points.push_back(point);
        }
    }

    return points;
}
//===================================================================


// 从OccupancyGrid生成地图的轮廓点云
std::vector<Point2D> generateMapOutline(const nav_msgs::OccupancyGrid& map) {
    std::vector<Point2D> map_points;

    int width = map.info.width;
    int height = map.info.height;
    float resolution = map.info.resolution;
    float origin_x = map.info.origin.position.x;
    float origin_y = map.info.origin.position.y;

    // 遍历地图的每一个格子
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            int index = x + y * width;
            int value = map.data[index];
            generateMapvalues_.push_back(value);

            // 检查是否是被占用的单元格
            if (map.data[index] > 50) { // 占用概率大于50表示被占用

                Point2D point;
                point.x = x * resolution + origin_x;
                point.y = y * resolution + origin_y;
                point.z = 0.0;
                map_points.push_back(point);
//==============================================================================
            }
        }
    }

    return map_points;
}



// 将点云数据从laser坐标系转换到map坐标系 tf::StampedTransform laser_to_map=transform
std::vector<Point2D> transformPointCloudToMap(const std::vector<Point2D>& cloud, const tf::Transform& transform) {
    std::vector<Point2D> transformed_cloud;
    for (const auto& point : cloud) {
        tf::Point tf_point(point.x, point.y, 0.0);
        tf::Point tf_point_transformed = transform * tf_point;
        Point2D transformed_point = {tf_point_transformed.x(), tf_point_transformed.y(),0.0};
        transformed_cloud.push_back(transformed_point);
    }
    return transformed_cloud;
}

//                std::cout << "p1.x:" << p1.x << " y:" << p1.y << std::endl;
//                std::cout << "p2.x:" << p2.x << " y:" << p2.y << std::endl;



// wx 和 wy：待转换的世界坐标 double 。mx 和 my：转换后的离散地图坐标。int像素坐标

//该函数将栅格地图坐标 int 转换为世界坐标 double。mx 和 my：待转换后的离散地图坐标，int像素坐标。 wx 和 wy：转换后的世界坐标。
void mapToWorld(double mx, double my, double& wx, double& wy, const nav_msgs::OccupancyGrid& map) {
    double resolution = map.info.resolution;
    double origin_x = map.info.origin.position.x;
    double origin_y = map.info.origin.position.y;

    wx = origin_x + (mx + 0.5) * resolution; // 偏移0.5是因为栅格中心
    wy = origin_y + (my + 0.5) * resolution; // 偏移0.5是因为栅格中心

}
//该函数将世界坐标double转换为栅格地图坐标int，wx 和 wy：待转换的世界坐标 double 。mx 和 my：转换后的离散地图坐标，像素坐标
bool worldToMap(double wx, double wy, double& mx, double& my, const nav_msgs::OccupancyGrid& map) {
    double resolution = map.info.resolution;
    double origin_x = map.info.origin.position.x;
    double origin_y = map.info.origin.position.y;

    mx = (wx - origin_x) / resolution - 0.5; // 偏移0.5是因为栅格中心
    my = (wy - origin_y) / resolution - 0.5; // 偏移0.5是因为栅格中心

    if (mx >= 0 && mx < map.info.width && my >= 0 && my < map.info.height) {
        return true;
    }
    return false;
}

// 将世界坐标 double 转换为栅格坐标int ，mx 和 my：转换后的离散地图坐标，int  像素坐标
bool worldToMapNew(double wx, double wy, double& mx, double& my, const nav_msgs::OccupancyGrid& map) {
    double origin_x = map.info.origin.position.x;
    double origin_y = map.info.origin.position.y;
    double resolution = map.info.resolution;

    if (wx < origin_x || wy < origin_y)
        return false;

    mx = (wx - origin_x) / resolution;
    my = (wy - origin_y) / resolution;

    mx = std::floor(mx);
    my = std::floor(my);

    if (mx >= 0 && my >= 0 && mx < map.info.width && my < map.info.height)
        return true;

    return false;
}

//================================================================================
// 从行优先数据中获取指定位置的值， 世界坐标 wx 和 wy：待转换的世界坐标 double 。result_Mapvalue：转换后的值，int
bool getMapValueWithWorld(double wx, double wy, int & result_Mapvalue, const nav_msgs::OccupancyGrid& map) {
    double mx, my;

    // 将世界坐标转换为栅格坐标
    if (!worldToMap(wx, wy, mx, my, map)) {
        return false;
    }
   // std::cout << " 01 将世界坐标转换为栅格坐标int  mapTbase_link: " << mx << ", " << my << std::endl;

    int ix = static_cast<int>(mx);
    int iy = static_cast<int>(my);

  //  std::cout << " 将世界坐标转换为栅格坐标int  mapTbase_link: ix,iy " << ix << ", " << iy << std::endl;

    // 计算一维数据中的索引
    int index = iy * map.info.width + ix;
  //  std::cout << " 02 将世界坐标转换为栅格坐标int  mapTbase_link: ix,iy " << ix << ", " << iy <<", index" << index <<  std::endl;
    // 确保索引在有效范围内
    if (index < 0 || index >= static_cast<int>(generateMapvalues_.size())) {
        return false;
    }
    result_Mapvalue=generateMapvalues_[index];
    result_Mapvalue_=result_Mapvalue;
  //  std::cout << " 03 将世界坐标转换为栅格坐标int  result_Mapvalue:  " <<result_Mapvalue<<  std::endl;
    // 返回对应的值
    return true;
}




// 从行优先数据中获取指定位置的值，像素坐标 mx 和 my：待转换的离散地图坐标，int 像素坐标
bool getMapValueWithMap(int mx, int my, int & result_Mapvalue, const nav_msgs::OccupancyGrid& map) {
    int index = my * map.info.width + mx;
    if (index < 0 || index >= static_cast<int>(generateMapvalues_.size())) {
        //throw std::out_of_range("Index out of bounds in map values vector.");
        return false;
    }
    result_Mapvalue=generateMapvalues_[index];
    result_Mapvalue_=result_Mapvalue;
    return true;
}

//=======================================检查激光雷达的数据是否与地图匹配======================================================

// 检查一个点在指定半径内是否有网格值为100的数据
bool isPointNearOccupied(int mx, int my, const nav_msgs::OccupancyGrid& map, double radius) {
    int radius_in_cells = static_cast<int>(std::ceil(radius / map.info.resolution));

    for (int dy = -radius_in_cells; dy <= radius_in_cells; ++dy) {
        for (int dx = -radius_in_cells; dx <= radius_in_cells; ++dx) {
            int nx = mx + dx;
            int ny = my + dy;

            if (nx >= 0 && ny >= 0 && nx < map.info.width && ny < map.info.height) {
                int value;
                bool valuefig = getMapValueWithMap(nx, ny, value, map);
                if (valuefig && value >= occupied_cell_) {
                   // std::cout << "找到匹配的点：" << nx << ", " << ny <<  ",value " << value << std::endl;
                    return true;
                }
            }
        }
    }

    return false;
}


//re.resultmatching_points_size=matching_points_;//匹配到的数据量
//re.scan_points_map_size=scan_points_map_size;//雷达点数据量
//re.isCalculateScanMapRatio=is_ratio_;//是否满足匹配要求


// 计算雷达点在地图中的匹配度
bool calculateScanMapRatio(const std::vector<Point2D>& scan_points_map, const nav_msgs::OccupancyGrid& map,
                           double radius, double threshold) {
    int total_points = 0;
    int matching_points = 0;

    for (const auto& point : scan_points_map) {
        double mx, my;
        if (worldToMap(point.x, point.y, mx, my, map)) {
            total_points++;
            if (isPointNearOccupied(static_cast<int>(mx), static_cast<int>(my), map, radius)) {
                matching_points++;
            }
        }
    }

//    if (total_points == 0){
//         total_points_ = 0;
//         matching_points_ = 0;
//         scan_points_map_size_=0;
//        return false;
//    }
    total_points_ = total_points;
    matching_points_ = matching_points;
    scan_points_map_size_=scan_points_map.size();

    double ratio =0;
    if(scan_points_map.size()!=0){
        //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
        // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
        if(is_use_scan_points_map_size_){
            ratio = static_cast<double>(matching_points) / scan_points_map.size();
        }else{
            ratio = static_cast<double>(matching_points) / total_points;
        }


    }


//    std::cout <<YELLOW <<  "计算雷达点在地图中的匹配度 547 calculateScanMapRatio 匹配上的雷达数据量 matching_points：" << matching_points<< ", 雷达点数据量的有效数据量 total_points:  " << total_points << " ,雷达数量 scan_points_map.size(): " << scan_points_map.size()  <<RESET<< std::endl;
//    if(ratio>=threshold){
//        std::cout <<YELLOW <<  "计算雷达点在地图中的匹配度 547 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold:  " << threshold  <<RESET<< std::endl;
//    }else{
//        std::cout <<RED <<  "计算雷达点在地图中的匹配度 548 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold:  " << threshold  <<RESET<< std::endl;
//    }


   ratio_=ratio;
    bool is_ratio = (ratio >= threshold);
    is_ratio_=is_ratio;
    return is_ratio;//0.99是最优秀的匹配比例
}


// 计算雷达点在地图中的匹配度
bool calculateScanMapRatio(const std::vector<Point2D>& scan_points_map, const nav_msgs::OccupancyGrid& map,
                           double radius, double threshold,int &resultmatching_points_size,int &scan_points_map_size,int &total_points_size,bool  &isCalculateScanMapRatio) {
    int total_points = 0;
    int matching_points = 0;

    for (const auto& point : scan_points_map) {
        double mx, my;
        if (worldToMap(point.x, point.y, mx, my, map)) {
            total_points++;
            if (isPointNearOccupied(static_cast<int>(mx), static_cast<int>(my), map, radius)) {
                matching_points++;
            }
        }
    }

//    if (total_points == 0){
//        total_points_ = 0;
//        matching_points_ = 0;
//        scan_points_map_size_=0;
//        return false;
//    }

    //---------------------------------------
    resultmatching_points_size=matching_points;
    scan_points_map_size=scan_points_map.size();
    total_points_size= total_points;
    double ratio =0;
    if(scan_points_map_size!=0){
        if(is_use_scan_points_map_size_){
             ratio = static_cast<double>(matching_points) / scan_points_map_size;
        }else{
            //double ratio = static_cast<double>(matching_points) / total_points;//计算雷达点数据量的有效数据量与地图轮廓的匹配率

            ratio = static_cast<double>(matching_points) / total_points_size;//计算雷达点数据量的有效数据量与地图轮廓的匹配率
        }


    }

    isCalculateScanMapRatio=ratio;

    std::cout <<GREEN <<  "计算雷达点在地图中的匹配度 547 calculateScanMapRatio 匹配上的雷达数据量 matching_points：" << matching_points<< ", 雷达点数据量的有效数据量 total_points:  " << total_points << " ,雷达数量 scan_points_map.size(): " << scan_points_map.size()  <<RESET<< std::endl;
    if(ratio>=threshold){
        std::cout <<GREEN <<  "计算雷达点在地图中的匹配度 547 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold:  " << threshold  <<RESET<< std::endl;
    }else{
        std::cout <<RED <<  "计算雷达点在地图中的匹配度 548 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold:  " << threshold  <<RESET<< std::endl;
    }


    ratio_=ratio;
    bool is_ratio = (ratio >= threshold);
    is_ratio_=is_ratio;
    return is_ratio;//0.99是最优秀的匹配比例
}

bool scan_CB(const sensor_msgs::LaserScan::ConstPtr& scan) {
    // 尝试非阻塞上锁
    std::unique_lock<std::mutex> lock(scancallback_mutex_, std::try_to_lock);
    if (!lock.owns_lock()) {
        // 如果锁不可用，直接跳过本次循环
        // std::cout<<"scan_CB 如果锁不可用，直接跳过本次循环"<<matchokReflectorPanelPoint_.size()<<std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        return false;
    }

    if(isPublish_matchcostmapwithscan_mssage_){

        if (!map_) {
            ROS_WARN("No map data received yet.");
            return false;
        }

        // 将数据转换为点云格式
        std::vector<Point2D> scan_points = convertToPoint2D(scan);



        // 获取laser到map的变换
        tf::StampedTransform laser_to_map;
        tf::StampedTransform base_link_to_map_transform;//mapTbase_link
        try {
            tf_listener_->waitForTransform(map_frame_id_, scan->header.frame_id, ros::Time(), ros::Duration(1.0));
            tf_listener_->lookupTransform(map_frame_id_, scan->header.frame_id, ros::Time(0), laser_to_map);
            std::this_thread::sleep_for(std::chrono::milliseconds(5));

            //todo  mapTbase_link
            tf_listener_->waitForTransform(map_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
            tf_listener_->lookupTransform(map_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_map_transform);


        } catch (tf::TransformException& ex) {
            ROS_WARN("Could not get transform from laser to map: %s", ex.what());
            return false;
        }

        Point2D point2D_MapTbase_link;
        point2D_MapTbase_link.x = base_link_to_map_transform.getOrigin().x();
        point2D_MapTbase_link.y = base_link_to_map_transform.getOrigin().y();

        //todo 1. 移动机器人所在位置是否是自由区
        int  result_Mapvalue=-3;
        // 从行优先数据中获取指定位置的值， 世界坐标 wx 和 wy：待转换的世界坐标 double 。result_Mapvalue：转换后的值，int
        bool  result_MapvalueFig =   getMapValueWithWorld(point2D_MapTbase_link.x, point2D_MapTbase_link.y,  result_Mapvalue, *map_);
//        if(result_MapvalueFig){
//            std::cout << "1. ok 移动机器人所在位置是否是自由区 mapTbase_link: result_Mapvalue " <<result_Mapvalue<< std::endl;
//            if(result_Mapvalue==0){
//                ROS_INFO("1.1 ok scanCallback 移动机器人所在位置是自由区");
//            }else{
//                ROS_WARN("1.2 error scanCallback 移动机器人所在位置不是自由区");
//            }
//        }
//        else{
//            std::cout << "1. error scanCallback 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< std::endl;
//        }

        // 创建消息实例
        fanguangbantrilateration::matchcostmapwithscan_result getResultMapvalue_result_msg;
        getResultMapvalue_result_msg.result_Mapvalue=result_Mapvalue;
        getResultMapvalue_result_pub_.publish(getResultMapvalue_result_msg);

       // ROS_INFO("================================================================================================");



        // 将激光雷达点云数据转换到map坐标系
        std::vector<Point2D> scan_points_map = transformPointCloudToMap(scan_points, laser_to_map);
        // 将激光雷达点云数据转换为PointCloud2并发布
        sensor_msgs::PointCloud2 scan_cloud_msg = convertToPointCloud2(scan_points_map);
        scan_points_pub_.publish(scan_cloud_msg);

        //todo 2. 雷达匹配情况
        bool is_match =calculateScanMapRatio(scan_points_map, *map_,radius_, threshold_);

//        if(is_match){
//            ROS_INFO("2.ok scanCallback 雷达匹配情况 The scan matches the map.");
//        }else{
//            ROS_WARN("2.error scanCallback 雷达匹配情况 The scan does not match the map.");
//        }
        fanguangbantrilateration::matchcostmapwithscan_result getCalculateScanMapRatio_result_msg;
        getCalculateScanMapRatio_result_msg.resulttotal_points_size=total_points_;
        getCalculateScanMapRatio_result_msg.scan_points_map_size=scan_points_map.size();
        getCalculateScanMapRatio_result_msg.resultmatching_points_size=matching_points_;
        getCalculateScanMapRatio_result_msg.isCalculateScanMapRatio=is_match;
        getCalculateScanMapRatio_result_pub_.publish(getCalculateScanMapRatio_result_msg);


    }//if(isPublish_matchcostmapwithscan_mssage_)


    // 发布初始位姿
    //  initial_pose_pub.publish(initial_pose);



    return true;
}


// 处理数据
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scanMsg) {
    // 创建一个线程并分离它，使其自动销毁
    std::thread([scanMsg]() {
        //  scan_CB(scanMsg);
        scan_CB(scanMsg);
    }).detach();

}


void updatefun() {
    std::cout<<"1.updatefun 开一个线程"<<std::endl;

    while (iswileTure_) {
        if(!iswileTure_){
            return;
        }
        ros::spinOnce();//防止ros通讯堵塞
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
}

//===================================



//查找请求类型对应的处理函数,默认使用函数名即为 request_handlers_ 中的 key
void processRequest(const std::string& request_type, const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& res) {
    // 查找请求类型对应的处理函数, 默认使用函数名即为 request_handlers_ 中的 key
    auto it = request_handlers_.find(request_type);
    if (it != request_handlers_.end()) {
        // 调用找到的处理函数并传递请求和响应对象
        it->second(req, res);
    } else {
        // 输出无效的请求类型错误信息
        std::cerr << "Invalid request type: " << request_type << std::endl;
        res.resultTxt="Invalid request type ";
        res.result="error";
        res.success=false;
    }
}
//接收用户指令的服务,"/trilateration_service"
bool matchCostmapWithScanServiceCallback(fanguangbantrilateration::matchcostmapwithscan_srvs::Request &req,
                                  fanguangbantrilateration::matchcostmapwithscan_srvs::Response &res)
{

    // 调用 processRequest 函数处理请求和响应， request_type 默认使用函数名即为 request_handlers_ 中的 key
    processRequest(req.request_type, req, res);
    return true; // Return true to indicate success
}


//===========================================

//todo 1.明天做一个发布 toptic ，可控制开关，对面要看时间，如果一段时间内没收到数据就不算。2.原地旋转坐标变化问题
//
// rosrun fanguangbantrilateration trilateration_matchCostmapWithScan_node

int main(int argc, char** argv) {
    setlocale(LC_ALL, "");  //中文
    ros::init(argc, argv, "trilateration_matchCostmapWithScan_node");
    ros::NodeHandle nh;
    ros::NodeHandle nnh("~");
    nnh.param<std::string>("map_frame_id", map_frame_id_, "map");
    nnh.param<std::string>("base_frame_id", base_link_frame_id_, "base_link");
    nnh.param<std::string>("odom_frame_id", odom_frame_id_, "odom");
    nnh.param<std::string>("laser_frame_id", laser_frame_id_, "laser");//会自动获取scan中的数据。base_scan laser
    nnh.param<std::string>("imu_frame_id", imu_frame_id_, "imu_link");

    nnh.param<std::string>("scan_toptic_name", scan_toptic_name_, "/scan");
    nnh.param<std::string>("map_toptic_name", map_toptic_name_, "/map");
    nnh.param<std::string>("scan_points_map_toptic_name", scan_points_map_toptic_name_, "/scan_points_map");
    nnh.param<std::string>("map_points_toptic_name", map_points_toptic_name_, "/map_points");
    nnh.param<std::string>("initialpose_toptic_name", initialpose_toptic_name_, "/initialpose");
    nnh.param<std::string>("service_name", service_name_, "/match_costmap_with_scan_service");
    nnh.param<std::string>("getResultMapvalue_result_toptic_name", getResultMapvalue_result_toptic_name_, "/match_costmap_resultMapvalue");
    nnh.param<std::string>("getCalculateScanMapRatio_result_toptic_name", getCalculateScanMapRatio_result_toptic_name_, "/match_costmap_calculateScanMapRatio");


    nnh.param<int>("free_cell", free_cell_, 0);// 代价地图 0代表空闲，自由区域，（机器人所行驶的位置）
    nnh.param<int>("occupied_cell", occupied_cell_, 100);//100代表占用 地图轮廓 障碍
    nnh.param<int>("unknown_cell", unknown_cell_, -1);// -1代表未知

    nnh.param<double>("matchingcostma_radius", radius_, 0.1);//搜索半径
    nnh.param<double>("matchingcostma_threshold", threshold_, 0.8);//比例阀值，超过这个比例认为雷达匹配的挺好

    //是否发布数据
    nnh.param<bool>("isPublish_matchcostmapwithscan_mssage", isPublish_matchcostmapwithscan_mssage_, true);
    ////true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。false 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。
    nnh.param<bool>("is_use_scan_points_map_size", is_use_scan_points_map_size_, false);
    // 创建tf监听器map_points_name_
    tf_listener_ = new tf::TransformListener;
    try {
        // scan_msg->header.frame_id
        tf_listener_->waitForTransform(base_link_frame_id_, laser_frame_id_, ros::Time(), ros::Duration(1.0));
        tf_listener_->lookupTransform(base_link_frame_id_, laser_frame_id_, ros::Time(0), base_link_to_laser_transform_);

        std::this_thread::sleep_for(std::chrono::milliseconds(5));

    } catch (tf::TransformException& ex) {
        std::cout <<"订阅 odom_to_map  发布 tf " << std::endl;
        ROS_WARN("%s", ex.what());

    }


    // 创建发布者 scanCallback 中的数据
    getResultMapvalue_result_pub_ = nh.advertise<fanguangbantrilateration::matchcostmapwithscan_result>(getResultMapvalue_result_toptic_name_, 10);
    getCalculateScanMapRatio_result_pub_ = nh.advertise<fanguangbantrilateration::matchcostmapwithscan_result>(getCalculateScanMapRatio_result_toptic_name_, 10);



    // 创建发布器
    scan_points_pub_ = nh.advertise<sensor_msgs::PointCloud2>(scan_points_map_toptic_name_, 1);
    std::string scan_points_map_toptic_name = scan_points_map_toptic_name_+"trilateration";
    //用三边定位转换雷达点云
    scan_points_trilateration_pub_ = nh.advertise<sensor_msgs::PointCloud2>(scan_points_map_toptic_name_, 1);
    //--------------------------------------------------------------------------------------------------

    ros::Publisher map_points_pub = nh.advertise<sensor_msgs::PointCloud2>(map_points_toptic_name_, 1,true);
    // 发布初始位姿
    initial_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(initialpose_toptic_name_, 1);




    // 等待地图数据
    ros::Duration timeout(10.0); // 超时时间10秒
    map_ = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(map_toptic_name_, nh, timeout);
    if (!map_) {
        ROS_ERROR("Failed to receive map data.");
        return -1;
    }

    // 转换并发布点云数据
    map_points_ = generateMapOutline(*map_);
    // 将地图轮廓点云数据转换为PointCloud2并发布
    sensor_msgs::PointCloud2 map_cloud_msg = convertToPointCloud2(map_points_);
    map_points_pub.publish(map_cloud_msg);

    // 订阅激光雷达数据
    ros::Subscriber scan_sub = nh.subscribe<sensor_msgs::LaserScan>(scan_toptic_name_, 1, scanCallback);

    // service 服务 /trilateration_service
    ros::ServiceServer service = nh.advertiseService(service_name_, matchCostmapWithScanServiceCallback);

    // 创建 service 服务 request_handlers_ 函数映射表
    request_handlers_ = createRequestHandlers();
    std::thread updatefun_thread(updatefun);

    ros::spin();
    if (updatefun_thread.joinable()) {
        updatefun_thread.join();
    }
    // 释放tf监听器
    delete tf_listener_;

    return 0;
}
// 获取地图代价值
void handleGetCurrentBaselinkResultMapvalue(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleGetCurrentBaselinkResultMapvalue..." << std::endl;


    tf::StampedTransform base_link_to_map_transform;//mapTbase_link
    try {

        //todo  mapTbase_link
        tf_listener_->waitForTransform(map_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
        tf_listener_->lookupTransform(map_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_map_transform);

    } catch (tf::TransformException& ex) {
        ROS_WARN("Could not get transform from laser to map: %s", ex.what());
        re.result_Mapvalue=-3;
        re.result="error";
        re.success=false;
        return;
    }

    Point2D point2D_MapTbase_link;
    point2D_MapTbase_link.x = base_link_to_map_transform.getOrigin().x();
    point2D_MapTbase_link.y = base_link_to_map_transform.getOrigin().y();

    //todo 1. 移动机器人所在位置是否是自由区
    int  result_Mapvalue=-3;
    // 从行优先数据中获取指定位置的值， 世界坐标 wx 和 wy：待转换的世界坐标 double 。result_Mapvalue：转换后的值，int
    bool  result_MapvalueFig =   getMapValueWithWorld(point2D_MapTbase_link.x, point2D_MapTbase_link.y,  result_Mapvalue, *map_);
    if(result_MapvalueFig){
        std::cout << "1. ok 移动机器人所在位置是否是自由区 mapTbase_link: result_Mapvalue " <<result_Mapvalue<< std::endl;
        if(result_Mapvalue==0){
            ROS_INFO("1.1 ok 移动机器人所在位置是自由区");
            re.result_Mapvalue=result_Mapvalue;
            // re.result_Mapvalue
            re.result="ok";
            re.success=true;
        }else{
            if(result_Mapvalue_==100){
                std::cout <<RED <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,障碍物,或地图轮廓"<<RESET<< std::endl;

            }else if(result_Mapvalue_==-1){
                std::cout <<YELLOW <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,未知区域"<<RESET<< std::endl;
            }
            re.result_Mapvalue=result_Mapvalue;
            re.result="error";
            re.success=false;
            return;
        }

    }else{

        if(result_Mapvalue_==100){
            std::cout <<RED <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,障碍物,或地图轮廓"<<RESET<< std::endl;

        }else if(result_Mapvalue_==-1){
            std::cout <<YELLOW <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,未知区域"<<RESET<< std::endl;
        }


        re.result="error";
        re.success=false;
        return;
    }

    //代价值
   // re.result_Mapvalue=result_Mapvalue_;
    re.result_Mapvalue=result_Mapvalue;
    // re.result_Mapvalue
    re.result="ok";
    re.success=true;
}

// 获取当前移动机器人的地图代价值（不需要传坐标过去，那边会通过tf获取当前机器人的位置）
void handleGetResultMapvalue(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleGetCurrentBaselinkResultMapvalue..." << std::endl;
    double x=req.request_posint_x;
    double y=req.request_posint_y;
    double z=req.request_posint_z;


    //todo 1. 移动机器人所在位置是否是自由区
    int  result_Mapvalue=-3;
    // 从行优先数据中获取指定位置的值， 世界坐标 wx 和 wy：待转换的世界坐标 double 。result_Mapvalue：转换后的值，int
    bool  result_MapvalueFig =   getMapValueWithWorld(x, y,  result_Mapvalue, *map_);
    if(result_MapvalueFig){
        if(result_Mapvalue_==0){
            std::cout <<GREEN <<  "1. ok 移动机器人所在位置是否是自由区 mapTbase_link: result_Mapvalue " <<result_Mapvalue<<  " ,自由区域"<<std::endl;
        }

        re.result_Mapvalue=result_Mapvalue;
        re.result="ok";
        re.success=true;
        return;
    }else{
        if(result_Mapvalue_==100){
            std::cout <<RED <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,障碍物,或地图轮廓"<<RESET<< std::endl;

        }else if(result_Mapvalue_==-1){
            std::cout <<YELLOW <<  "1. error 移动机器人所在位置是否是自由区 mapTbase_link :result_Mapvalue " << result_Mapvalue<< " ,未知区域"<<RESET<< std::endl;
        }

        re.result_Mapvalue=-3;
        re.result="error";
        re.success=true;
        return;
    }

    //re.result_Mapvalue=result_Mapvalue_;
    re.result_Mapvalue=result_Mapvalue;
    re.result="ok";
    re.success=true;
}


//-------------------------------------------------

//获取激光雷达在地图上的匹配情况，获取激光雷达的点数，及激光雷达在地图轮廓上的点数，request_radius 搜索半径，request_threshold 比例阀值，超过这个比例认为雷达匹配的挺好
void handleGetCurrentCalculateScanMapRatio(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleGetCurrentCalculateScanMapRatio..." << std::endl;
    radius_=req.request_radius;//搜索半径
    threshold_=req.request_threshold;//比例阀值，超过这个比例认为雷达匹配的挺好
  //  std::this_thread::sleep_for(std::chrono::milliseconds(50));

     re.resultmatching_points_size=matching_points_;//匹配到的数据量
     re.scan_points_map_size=scan_points_map_size_;//雷达点数据量
     re.isCalculateScanMapRatio=is_ratio_;//是否满足匹配要求


    double ratio =0;
    //if(scan_points_map_size_!=0|| total_points_size_!=0){
    if(scan_points_map_size_!=0){
        //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
        // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
        if(is_use_scan_points_map_size_){
            ratio = static_cast<double>(matching_points_) / scan_points_map_size_;
        }else{
            ratio = static_cast<double>(matching_points_) / total_points_;//计算雷达点数据量的有效数据量与地图轮廓的匹配率
        }



    }


    re.result="ok";
    re.success=true;

    std::cout <<GREEN << "handleGetCurrentCalculateScanMapRatio 895 匹配上的雷达数据量 matching_points：" << matching_points_<< ", 有效雷达数量 total_points:  " << total_points_ << " ,雷达数量 scan_points_map.size(): " << scan_points_map_size_ <<  RESET << std::endl;

    if(ratio>=threshold_){
        std::cout <<GREEN <<  "handleGetCurrentCalculateScanMapRatio 896 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold_:  " << threshold_  <<RESET<< std::endl;

    }else{
        std::cout <<RED <<  "handleGetCurrentCalculateScanMapRatio 896 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold_:  " << threshold_  <<RESET<< std::endl;
    }


}


//获取激光雷达在地图上的匹配情况，获取激光雷达的点数，及激光雷达在地图轮廓上的点数，request_radius 搜索半径，request_threshold 比例阀值，超过这个比例认为雷达匹配的挺好
void handleGetCalculateScanMapRatio(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleGetCalculateScanMapRatio..." << std::endl;

    radius_=req.request_radius;//搜索半径
    threshold_=req.request_threshold;//比例阀值，超过这个比例认为雷达匹配的挺好
    //  std::this_thread::sleep_for(std::chrono::milliseconds(50));
    //通过三边定位获取移动机器人坐标
    geometry_msgs::Pose request_mapTbase_trilateration_transform=req.request_mapTbase_trilateration_transform;
    ros::topic::waitForMessage<sensor_msgs::LaserScan>(scan_toptic_name_, ros::Duration(5));

    try {
        boost::shared_ptr<sensor_msgs::LaserScan  const>    scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>(scan_toptic_name_, ros::Duration(0.5));
        if (scan_msg)
        {
            // 转换 Pose 到 Transform
            tf::Transform mapTbase_link_trilateration_transform = poseToTransform(request_mapTbase_trilateration_transform);
            //mapTlaser
            tf::Transform laser_to_map =    mapTbase_link_trilateration_transform*base_link_to_laser_transform_.inverse();

            // 将数据转换为点云格式
            std::vector<Point2D> scan_points = convertToPoint2D(scan_msg);
            // 将激光雷达点云数据转换到map坐标系
            std::vector<Point2D> scan_points_map = transformPointCloudToMap(scan_points, laser_to_map);
            // 将激光雷达点云数据转换为PointCloud2并发布
            sensor_msgs::PointCloud2 scan_cloud_msg = convertToPointCloud2(scan_points_map);
            scan_points_trilateration_pub_.publish(scan_cloud_msg);

            int matching_points=0;//匹配到的数据量
            int scan_points_map_size=0;//雷达点数据量
            int total_points_size=0;//雷达点数据量的有效数据量
            bool is_ratio=false;//是否满足匹配要求

            //todo 2. 雷达匹配情况
            bool is_match =calculateScanMapRatio(scan_points_map, *map_,radius_, threshold_,matching_points,scan_points_map_size,total_points_size,is_ratio);
            re.resultmatching_points_size=matching_points;//匹配到的数据量
            re.scan_points_map_size=scan_points_map_size;//雷达点数据量
            re.resulttotal_points_size=total_points_size;//雷达点数据量的有效数据量
            re.isCalculateScanMapRatio=is_match;//是否满足匹配要求


            double ratio =0;
            //if(scan_points_map_size_!=0|| total_points_size_!=0){
            if(scan_points_map_size!=0){
                //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
                // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
                if(is_use_scan_points_map_size_){
                           ratio = static_cast<double>(matching_points) / scan_points_map_size;
                }else{
                    ratio = static_cast<double>(matching_points) / total_points_size;//计算雷达点数据量的有效数据量与地图轮廓的匹配率
                }


            }

            std::cout <<GREEN << "handleGetCalculateScanMapRatio 968  匹配上的雷达数据量 matching_points：" << matching_points<< ", 有效雷达数量 total_points:  " << total_points_size << " ,雷达数量 scan_points_map.size(): " << scan_points_map_size  <<RESET <<  std::endl;

            if(ratio>=threshold_){
                std::cout <<GREEN <<  "handleGetCalculateScanMapRatio 969  匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 threshold_:  " << threshold_  <<RESET<< std::endl;

            }else{
                std::cout <<RED <<  "handleGetCalculateScanMapRatio 969  匹配上的雷达数有效比例  ratio：" << ratio<< ", 比例 阀值 threshold_:  " << threshold_  <<RESET<< std::endl;
            }

                re.result="ok";
                re.success=true;
                return;
        }//if (scan_msg)
       else{

            re.resultmatching_points_size=0;//匹配到的数据量
            re.scan_points_map_size=0;//雷达点数据量
            re.resulttotal_points_size=0;//雷达点数据量的有效数据量
            re.isCalculateScanMapRatio=false;//是否满足匹配要求


            std::cout <<RED << "handleGetCalculateScanMapRatio 968  匹配上的雷达数据量 matching_points：" << 0<< ", 有效雷达数量 total_points:  " << 0 << " ,雷达数量 scan_points_map.size(): " << 0  <<RESET <<  std::endl;
            std::cout <<RED <<  "handleGetCalculateScanMapRatio 969  匹配上的雷达数有效比例  ratio：" << 0<< ", 比例 阀值 threshold_:  " << threshold_  <<RESET<< std::endl;



            re.result="error";
            re.success=true;
            return;
       }




    } catch (tf::TransformException& ex) {
        ROS_WARN("Could not get transform from laser to map: %s", ex.what());
        re.result="error";
        re.resultmatching_points_size=0;//匹配到的数据量
        re.scan_points_map_size=0;//雷达点数据量
        re.resulttotal_points_size=0;//雷达点数据量的有效数据量
        re.isCalculateScanMapRatio=false;//是否满足匹配要求

        re.result="error";
        re.success=true;
        return;
    }


    re.result="ok";
    re.success=true;






}

// 是否发布数据
void handleIsPublish_matchcostmapStartOrStop(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleIsPublish_matchcostmapStartOrStop..." << std::endl;
    isPublish_matchcostmapwithscan_mssage_= req.request_isPublish_matchcostmapwithscan_mssage;
    is_use_scan_points_map_size_= req.request_is_use_scan_points_map_size;
    // re.result_Mapvalue
    re.result="ok";
    re.success=true;
}

//---------------------------------------------------------
// 设置参数
void handleSetParam(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout << "handleSetParam..." << std::endl;
    isPublish_matchcostmapwithscan_mssage_=req.request_isPublish_matchcostmapwithscan_mssage;
    is_use_scan_points_map_size_= req.request_is_use_scan_points_map_size;
    free_cell_=req.request_free_cell ;
    occupied_cell_=req.request_occupied_cell ;
    unknown_cell_=req.request_unknown_cell ;
    radius_=req.request_radius;
    threshold_=req.request_threshold;
    re.result="ok";
    re.success=true;
}
//检查此节点是否启动正常工作
void handleCheckNodeStart(const fanguangbantrilateration::matchcostmapwithscan_srvs::Request& req, fanguangbantrilateration::matchcostmapwithscan_srvs::Response& re) {
    std::cout <<YELLOW<< "此节点 match_costmap_with_scan_node 正常工作   handleCheckNodeStart   ..." <<RESET<< std::endl;
    re.result="ok";
    re.success=true;
}



//创建一个函数，用于填充 request_handlers 映射表
std::map<std::string, HandlerFunction> createRequestHandlers() {
    std::map<std::string, HandlerFunction> handlers;
    //用于自动生成键值对
    ADD_HANDLER(handlers, handleGetResultMapvalue);//获取地图代价值
    ADD_HANDLER(handlers, handleGetCurrentBaselinkResultMapvalue);//获取地图代价值
    ADD_HANDLER(handlers, handleGetCalculateScanMapRatio);//获取激光雷达在地图上的匹配情况，获取激光雷达的点数，及激光雷达在地图轮廓上的点数，request_radius 搜索半径，request_threshold 比例阀值，超过这个比例认为雷达匹配的挺好
    ADD_HANDLER(handlers, handleGetCurrentCalculateScanMapRatio);
    ADD_HANDLER(handlers, handleIsPublish_matchcostmapStartOrStop);// 是否发布数据
    ADD_HANDLER(handlers, handleSetParam);// 设置参数
    ADD_HANDLER(handlers, handleCheckNodeStart);// 检查此节点是否启动正常工作
    return handlers;
}
